#include "util.h"
#include <math.h>

#define A (sqrt(5.0)-1)/2
#ifndef M_PI
    #define M_PI
#endif

double wrap_angle(double a)
{
    a = fmod((float)(a + M_PI), (float)(2 * M_PI));
    if (a < 0.0)
        a += 2.0 * M_PI;
    return a - M_PI;
}

float first_order_filter(float sampling_data, float last_result, float coefficient)
{
    float result = coefficient * sampling_data + (1 - coefficient) * last_result;
    return result;
}

// float sum(float *array, short size)
// {
//     float s = 0;
//     for (int i = 0; i < size; i++)
//     {
//         s += array[i];
//     }
//     return s;
// }

// float mean(float *array, short size)
// {
//     float s = sum(array, size);
//     return (s / size);
// }

// float variance(float *array, short size)
// {
//     float var = 0;
//     float m = mean(array, size);

//     for (int i = 0; i < size; i++)
//     {
//         float temp = array[i] - m;
//         var += temp * temp;
//     }
//     var /= size;
//     return var;
// }

float mean(float last_avg, float new_data, float old_data, short size)
{
    float new_avg = last_avg + (new_data - old_data) / size;
    return new_avg;
}

float variance(float last_var, float last_avg, float new_data, float old_data, short size)
{
    float diff = new_data - old_data;
    float new_var = last_var + ((new_data + old_data - 2 * last_avg) - diff / size) * diff / size;
    return new_var;
}

void endian_mode_transform_4bytes(unsigned char *input, unsigned char *output)
{
    output[3] = input[0];
    output[2] = input[1];
    output[1] = input[2];
    output[0] = input[3];
}

void endian_mode_transform_2bytes(unsigned char *input, unsigned char *output)
{
    output[1] = input[0];
    output[0] = input[1];
}

unsigned int hash_fun(unsigned int key)
{
    double k = key;
    double x = k * A;
    double y = floor(k * A);
    return (2 << 28) * (x - y);
}
